/**
  ******************************************************************************
  * @file    operators.c
  * @author  Amaseing team
  * @version V1.0.0
  * @date    21-november-2013
  * @brief   Vision operators
  *         
  ******************************************************************************  
  */ 
  
/* Private includes ----------------------------------------------------------*/ 
#include "operators.h"
#include <stdlib.h>

/* Private declarations ------------------------------------------------------*/
/* Private prototypes --------------------------------------------------------*/
/* Private defines -----------------------------------------------------------*/
/* Functions -----------------------------------------------------------------*/
void camConvertLumaToGrayScale(image_t *img){
	float r, g, b;
	uint32_t pixdata = (img->height*(img->width/2));
	yuv32_t *imgPtr = (yuv32_t *)img->data;
	uint8_t *grayPtr = ((uint8_t *)img->data)+1;
	yuv32_t temp;

	while(pixdata--)
	{
		//uneven pixel
		temp = *imgPtr++;
		r = temp.Y0 + temp.Cr0;
		g = temp.Y0 - (0.166667f*temp.Cb0) - (0.5f*temp.Cr0);
		b = temp.Y0 + temp.Cb0;
		
		//*grayPtr++ = (uint16_t)((r*0.3f) + (g*0.59f) + (b*0.11f));
		*grayPtr = (uint8_t)((r*0.3f) + (g*0.59f) + (b*0.11f));
		grayPtr+=2;
		
		
		//even pixel
		r = temp.Y1 + temp.Cr0;
		g = (temp.Y1 - (0.166667f*temp.Cb0) - (0.5f*temp.Cr0));
		b = temp.Y1 + temp.Cb0;
		
		//*grayPtr++ = (uint8_t)((r*0.3f) + (g*0.59f) + (b*0.11f));
		*grayPtr = (uint8_t)((r*0.3f) + (g*0.59f) + (b*0.11f));
		grayPtr+=2;
	}
}

void vCalculateMeanOfThreeRows(image_t *img){
	// pixelPtr,pixelPtr2,pixelPtr3 are used for rows 0, 1 and 2 respectively.
	uint32_t count = img->width/2;
	yuv32_t *pixelResult = img->data;
	yuv32_t *pixelPtr = img->data;
	yuv32_t *pixelPtr2 = pixelPtr + (img->width/2);
	yuv32_t *pixelPtr3 = pixelPtr + ((img->width/2)*2);
	
	while(count--) {
		// Fetch the column of 3 pixels
		yuv32_t pixel[3] = {*(pixelPtr++), *(pixelPtr2++), *(pixelPtr3++)};
		
		// Calculate average value for each channel
		int Cb0, Y0, Cr0, Y1;
		Cb0 = pixel[0].Cb0 + pixel[1].Cb0 + pixel[2].Cb0;
		Cb0 /= 3;
		Y0 = pixel[0].Y0 + pixel[1].Y0 + pixel[2].Y0;
		Y0 /= 3;
		Cr0 = pixel[0].Cr0 + pixel[1].Cr0 + pixel[2].Cr0;
		Cr0 /= 3;
		Y1 = pixel[0].Y1 + pixel[1].Y1 + pixel[2].Y1;
		Y1 /= 3;
		
		// Write result.
		yuv32_t resultPixel = { Cb0, Y0, Cr0, Y1 };
		*(pixelResult++) = resultPixel;
	}
}
static inline void sort9(uint32_t *array){
	static uint32_t i, j;
	static uint32_t tmp;
	
	for(i=1; i<9; i++){
		tmp = array[i];
		for(j=i; j >= 1 && tmp < array[j-1]; j--)
			array[j] = array[j-1];
		array[j] = tmp;
	}
}
void vMedianYUVLumaFilter(image_t *img){
	uint32_t Mask[9] = {0};

	// filter Y0 and Y1
	uint32_t pixels = ((img->height-2)*(img->width-2));
	uint32_t width = img->width-2;
	uint32_t tmp;
	
	uint16_t *pData1 = ((uint16_t*)img->data);
	uint16_t *pData2 = ((uint16_t*)img->data) + (img->width);
	uint16_t *pData3 = ((uint16_t*)img->data) + (2*img->width);
	uint16_t *pDataRes = pData2 + 1;
	
	//init mask
	Mask[0] = *pData1++;
	Mask[1] = *pData1++;
	Mask[2] = *pData1++;
	Mask[3] = *pData2++;
	Mask[4] = *pData2++;
	Mask[5] = *pData2++;
	Mask[6] = *pData3++;
	Mask[7] = *pData3++;
	Mask[8] = *pData3++;
		
	while(pixels--){
		//sort the values
		sort9(Mask);
		
		//write median to the result pointer
		tmp = *pDataRes;
		tmp &= (tmp&0x000000FF);
		tmp |= (Mask[4]&0x0000FF00);
		*pDataRes = tmp;
		
		//continue in row or goto next row		
		if(pixels%width){ 
			pDataRes++;			
			
			Mask[0] = Mask[1];
			Mask[1] = Mask[2];
			Mask[2] = *pData1++;
			
			Mask[3] = Mask[4];
			Mask[4] = Mask[5];
			Mask[5] = *pData2++;;
			
			Mask[6] = Mask[7];
			Mask[7] = Mask[8];
			Mask[8] = *pData3++;
		}
		else //goto next row
		{
			if(pixels >= width){ //last row? no; proceed
				pDataRes+=3;			
				
				//init masker for next row
				Mask[0] = *pData1++;
				Mask[1] = *pData1++;
				Mask[2] = *pData1++;
				Mask[3] = *pData2++;
				Mask[4] = *pData2++;
				Mask[5] = *pData2++;
				Mask[6] = *pData3++;
				Mask[7] = *pData3++;
				Mask[8] = *pData3++;
			}
		}
	}
}
void vMedianYUVColorFilter(image_t *img){
	uint32_t Mask[9] = {0};

	// filter Cb
	uint32_t pixels = ((img->height-2)*((img->width/2)-2));
	uint32_t width = ((img->width/2)-2);
	
	yuv32_t *pData1 = ((yuv32_t*)img->data);
	yuv32_t *pData2 = ((yuv32_t*)img->data) + (img->width/2);
	yuv32_t *pData3 = ((yuv32_t*)img->data) + (2*(img->width/2));
	yuv32_t *pDataRes = pData2 + 1;
	
	//init mask
	Mask[0] = pData1++->Cb0;
	Mask[1] = pData1++->Cb0;
	Mask[2] = pData1++->Cb0;
	Mask[3] = pData2++->Cb0;
	Mask[4] = pData2++->Cb0;
	Mask[5] = pData2++->Cb0;
	Mask[6] = pData3++->Cb0;
	Mask[7] = pData3++->Cb0;
	Mask[8] = pData3++->Cb0;
	
	while(pixels--){
		//sort the values
		sort9(Mask);
		
		//write median to the result pointer
		pDataRes->Cb0 = (Mask[4]&0x000000FF);

		//continue in row or goto next row		
		if(pixels%width){ 
			pDataRes++;			
			
			Mask[0] = Mask[1];
			Mask[1] = Mask[2];
			Mask[2] = pData1++->Cb0;
			
			Mask[3] = Mask[4];
			Mask[4] = Mask[5];
			Mask[5] = pData2++->Cb0;
			
			Mask[6] = Mask[7];
			Mask[7] = Mask[8];
			Mask[8] = pData3++->Cb0;
		}
		else //goto next row
		{
			if(pixels >= width){ //last row? no; proceed
				pDataRes+=3;			
				
				//init masker for next row
				Mask[0] = pData1++->Cb0;
				Mask[1] = pData1++->Cb0;
				Mask[2] = pData1++->Cb0;
				Mask[3] = pData2++->Cb0;
				Mask[4] = pData2++->Cb0;
				Mask[5] = pData2++->Cb0;
				Mask[6] = pData3++->Cb0;
				Mask[7] = pData3++->Cb0;
				Mask[8] = pData3++->Cb0;
			}
		}
	}
	
	// filter Cr
	pixels = ((img->height-2)*((img->width/2)-2));
	width = ((img->width/2)-2);
	
	pData1 = ((yuv32_t*)img->data);
	pData2 = ((yuv32_t*)img->data) + (img->width/2);
	pData3 = ((yuv32_t*)img->data) + (2*(img->width/2));
	pDataRes = pData2 + 1;
	
	//init mask
	Mask[0] = pData1++->Cr0;
	Mask[1] = pData1++->Cr0;
	Mask[2] = pData1++->Cr0;
	Mask[3] = pData2++->Cr0;
	Mask[4] = pData2++->Cr0;
	Mask[5] = pData2++->Cr0;
	Mask[6] = pData3++->Cr0;
	Mask[7] = pData3++->Cr0;
	Mask[8] = pData3++->Cr0;
	
	while(pixels--){
		//sort the values
		sort9(Mask);
		
		//write median to the result pointer
		pDataRes->Cr0 = (Mask[4]&0x000000FF);
		
		//continue in row or goto next row		
		if(pixels%width){ 
			pDataRes++;			
			
			Mask[0] = Mask[1];
			Mask[1] = Mask[2];
			Mask[2] = pData1++->Cr0;
			
			Mask[0] = Mask[1];
			Mask[1] = Mask[2];
			Mask[2] = pData2++->Cr0;
			
			Mask[0] = Mask[1];
			Mask[1] = Mask[2];
			Mask[2] = pData3++->Cr0;
		}
		else //goto next row
		{
			if(pixels >= width){ //last row? no; proceed
				pDataRes+=3;			
				
				//init masker for next row
				Mask[0] = pData1++->Cr0;
				Mask[1] = pData1++->Cr0;
				Mask[2] = pData1++->Cr0;
				Mask[3] = pData2++->Cr0;
				Mask[4] = pData2++->Cr0;
				Mask[5] = pData2++->Cr0;
				Mask[6] = pData3++->Cr0;
				Mask[7] = pData3++->Cr0;
				Mask[8] = pData3++->Cr0;
			}
		}
	}
}

void vHistogram(image_t  *src,
                uint16_t *hist){
	uint32_t i, totPixels = ((src->width/2)*src->height);
	yuv32_t pixel;
	yuv32_t *pDataSrc = (yuv32_t *)src->data;
	
	// all positions set to zero 
	for(i=0;i<256;i++)
			hist[i]=0;
			
	// calc histogram depending of field
	for(i=totPixels;i>0;i--){
			pixel = *pDataSrc++;
			hist[pixel.Y0]++;
			hist[pixel.Y1]++;
	}
}

void vThreshold(image_t *src,
                image_t *dst, // result is a binary image
                uint8_t low,
                uint8_t high){
									
	uint32_t totPixels = ((src->width/2)*src->height);
	yuv32_t temp;
	yuv32_t *pDataSrc = (yuv32_t *)src->data;
	yuv32_t *pDataDst = (yuv32_t *)dst->data;
	
	if(low<high){
		while(totPixels--){
				temp = *pDataSrc++;
				if(temp.Y0 >= low && temp.Y0 <= high){
						pDataDst->Y0 = 1;
				}
				else{
						pDataDst->Y0 = 0;
				}
				if(temp.Y1 >= low && temp.Y1 <= high){
						pDataDst->Y1 = 1;
				}
				else{
						pDataDst->Y1 = 0;
				}
				pDataDst->Cb0 = 0;
				pDataDst->Cr0 = 0;
				pDataDst++;
		}
	}
	else
	{
		while(totPixels--){
				temp = *pDataSrc++;
				if(temp.Y1 >= high && temp.Y1 <= low){
						pDataDst->Y0 = 0;
				}
				else{
						pDataDst->Y0 = 1;
				}
				if(temp.Y1 >= high && temp.Y1 <= low){
						pDataDst->Y1 = 0;
				}
				else{
						pDataDst->Y1 = 1;
				}
				pDataDst->Cb0 = 0;
				pDataDst->Cr0 = 0;
				pDataDst++;
		}
	}
}

void vThresholdIsoData(image_t *src,
                       image_t *dst,
                       uint32_t brightness){ // DARK | BRIGHT/LIGHT
												 
    uint16_t hist[256] = {0};
    uint32_t i,j,T, count;
    uint32_t meanL=0, meanR=0, meanM=0;

    vHistogram(src, hist);

    for(i=0;i<256;i++){
        if(hist[i] > 0){
            T = i;
            break;
        }
    }
    for(i=255;i>T;i--){
        if(hist[i] > 0){
            T = (T+i)/2;
            break;
        }
    }

    for(j=20;j>0;j--){//while(1)
        // Calculate meanL
        meanL = 0;
        count=0;
        for(i=0;i<(T+1);i++){ //{T+1 = perfomance!!, saves 1 loop
            meanL+=(hist[i]*i);
            count+=hist[i];
        }
        if(meanL != 0 || count != 0)
            meanL /= count;

        // Calculate meanR
        meanR = 0;
        count=0;
        for(i=T;i<256;i++){
            meanR+=(hist[i]*i);
            count+=hist[i];
        }
        if(meanR != 0 || count != 0)
            meanR /= count;

        // Calculate meanM 
        meanM = (meanL+meanR)/2;

        if(T == meanM)
            break;
        else
            T = meanM;
    }

    if(brightness != BRIGHT)
        vThreshold(src,dst,255,meanM);
    else
        vThreshold(src,dst,meanM,255);
}
void vThresholdYUVColor(image_t *img,
												enum eCOLOR color){
    uint32_t totPixels = ((img->width/2)*img->height);
    yuv32_t temp;
    yuv32_t *pDataImg = (yuv32_t *)img->data;
		uint32_t lowCb = colorconst[color].lowCb;
		uint32_t lowCr  = colorconst[color].lowCr;
		uint32_t highCb  = colorconst[color].highCb;
		uint32_t highCr  = colorconst[color].highCr;
		
		while(totPixels--){
				temp = *pDataImg;
				if(temp.Cb0 >= lowCb){
					if(temp.Cb0 <= highCb){
						if(temp.Cr0 >= lowCr){
							if(temp.Cr0 <= highCr){
								pDataImg->Y0 = 1;
								pDataImg->Y1 = 1;
							}
							else{
									pDataImg->Y0 = 0;
									pDataImg->Y1 = 0;
							}
						}
						else{
							pDataImg->Y0 = 0;
							pDataImg->Y1 = 0;
						}
					}
					else{
						pDataImg->Y0 = 0;
						pDataImg->Y1 = 0;
					}
				}
				else{
						pDataImg->Y0 = 0;
						pDataImg->Y1 = 0;
				}
				pDataImg->Cb0 = 0;
				pDataImg->Cr0 = 0;
				pDataImg++;
		}
}
int iScanRowForColor(image_t *img, 
											enum eCOLOR color, 
											colorobject_t *obj){
	yuv32_t* pixelPtr = img->data;
	yuv32_t pixel;
	uint32_t pixels = (img->width/2);
	uint32_t start=0, end=0;
	colorconstants_t temp;
	
	if(color >= COLOR_SIZE) //error
		return -1;
	
	temp = colorconst[color];
	
	while(pixels--){
		pixel = *pixelPtr++;
		if(pixel.Y0 >= temp.lowluma)
		{
			if(pixel.Y0 <= temp.highluma)
			{
				if(pixel.Cb0 >= temp.lowCb)
				{
					if(pixel.Cr0 >= temp.lowCr)
					{
						if(pixel.Cb0 <= temp.highCb)
						{
							if(pixel.Cr0 <= temp.highCr)
							{
								if(start==0)
								{
									if(pixelPtr->Cb0 >= temp.lowCb)
									{
										if(pixelPtr->Cr0 >= temp.lowCr)
										{
											if(pixelPtr->Cb0 <= temp.highCb)
											{
												if(pixelPtr->Cr0 <= temp.highCr)
												{
													start = pixels;
													end = pixels;
												}
											}
										}
									}
								}
								else
								{
									if(end == (pixels+1))
										end = pixels;
									else if (end == (pixels+2))
										end = pixels;
									else if (end == (pixels+3))
										end = pixels;
								}
							}
						}
					}
				}
			}
		}
	}
	if(end < start){		
		
		//convert to real pixel
		start = ((img->width) - (start*2));
		end = ((img->width) - (end*2));
		
		if((end - start) >= (COLOR_OBJECT_SIZE_MIN)){ //check minimum size of color object				
			if(obj != NULL) {
				obj->color = color;
				obj->xstart = start;
				obj->xstop = end;
				obj->ystart = ((DCMI->CWSTRTR&0x1FFF0000)>>16);
				obj->ystop = obj->ystart;
				return 1;
			}
			return 1;
		}
	}
	return 0;
}
int iScanRowForColors(image_t *img, 
											colorobject_t *obj,
											int8_t size){
	yuv32_t* pixelPtr;
	yuv32_t pixel;
	uint32_t pixels, start, end;
	uint32_t colorcnt = (COLOR_SIZE);
	uint32_t colorsDetected = 0;
	colorconstants_t temp; 
		
	while(colorcnt--){
		pixelPtr = img->data;
		pixels = (img->width/2);
		start = 0; 
		end = 0;
		temp = colorconst[colorcnt];
		
		while(pixels--){
			pixel = *pixelPtr++;
			if(pixel.Y0 >= temp.lowluma)
			{
				if(pixel.Y0 <= temp.highluma)
				{
					if(pixel.Cb0 >= temp.lowCb)
					{
						if(pixel.Cr0 >= temp.lowCr)
						{
							if(pixel.Cb0 <= temp.highCb)
							{
								if(pixel.Cr0 <= temp.highCr)
								{
									if(start==0)
									{
										if(pixelPtr->Cb0 >= temp.lowCb)
										{
											if(pixelPtr->Cr0 >= temp.lowCr)
											{
												if(pixelPtr->Cb0 <= temp.highCb)
												{
													if(pixelPtr->Cr0 <= temp.highCr)
													{
														start = pixels;
														end = pixels;
													}
												}
											}
										}
									}
									else
									{
										if(end == (pixels+1))
											end = pixels;
										else if (end == (pixels+2))
											end = pixels;
										else if (end == (pixels+3))
											end = pixels;
									}
								}
							}
						}
					}
				}
			}
		}

		if(end < start){		
			
			//convert to real pixel
			start = ((img->width) - (start*2));
			end = ((img->width) - (end*2));
			
			if((end - start) >= (COLOR_OBJECT_SIZE_MIN)){ //check minimum size of color object				
				if(colorsDetected<=size) {
					obj[colorsDetected].color = colorcnt;
					obj[colorsDetected].xstart = start;
					obj[colorsDetected].xstop = end;
					obj[colorsDetected].ystart = ((DCMI->CWSTRTR&0x1FFF0000)>>16);
					obj[colorsDetected].ystop = obj[colorsDetected].ystart;
				}
				colorsDetected++;
			}
		}
	}
	
	return colorsDetected;
}
int iScanColumnForColors(image_t *img, 
											enum eCOLOR color, 
											colorobject_t *obj){
	yuv32_t* pixelPtr = img->data;
	yuv32_t pixel;
	uint32_t pixels = ((img->width/2) * img->height);
	uint32_t pixels2 = ((img->width/2) * img->height);
	uint32_t start=0, end=0;
	colorconstants_t temp;
	
	if(color >= COLOR_SIZE) //error
		return -1;
	
	temp = colorconst[color];
	
	while(pixels--){
		pixel = *pixelPtr++;
		if(pixel.Y0 >= temp.lowluma)
		{
			if(pixel.Y0 <= temp.highluma)
			{
				if(pixel.Cb0 >= temp.lowCb)
				{
					if(pixel.Cr0 >= temp.lowCr)
					{
						if(pixel.Cb0 <= temp.highCb)
						{
							if(pixel.Cr0 <= temp.highCr)
							{
								if(start==0)
								{
									if(pixelPtr->Cb0 >= temp.lowCb)
									{
										if(pixelPtr->Cr0 >= temp.lowCr)
										{
											if(pixelPtr->Cb0 <= temp.highCb)
											{
												if(pixelPtr->Cr0 <= temp.highCr)
												{
													start = pixels;
													end = pixels;
												}
											}
										}
									}
								}
								else
								{
									if(end == (pixels+1))
										end = pixels;
									else if (end == (pixels+2))
										end = pixels;
									else if (end == (pixels+3))
										end = pixels;
								}
							}
						}
					}
				}
			}
		}
	}
	if(end < start){		
		
		//convert to real pixel
		start = ((pixels2-start)/2);
		end = ((pixels2 - end)/2);
				
		if((end - start) >= (COLOR_OBJECT_SIZE_MIN)){ //check minimum size of color object				
			if(obj != NULL) {
				obj->color = color;
				
				obj->ystart = start;
				obj->ystop = end;
				
				return 1;
			}
			return 1;
		}
	}
	return 0;
}
void vSetSelectedToValue(image_t *src,
                         image_t *dst,
                         uint8_t selected,
                         uint8_t value){
    uint32_t totPixels = ((src->width/2)*src->height);
    yuv32_t *pDataSrc, *pDataDst;
    yuv32_t temp;

    pDataSrc = (yuv32_t *)src->data;
    pDataDst = (yuv32_t *)dst->data;

    while(totPixels--)
    {
        temp = *pDataSrc++;
				
        if(temp.Y0 == selected)
					pDataDst->Y0 = value;
					
				if(temp.Y1 == selected)
					pDataDst->Y1 = value;
					
				pDataDst++;
    }
}
void vFillHoles(image_t *src, // must be a binary image
                image_t *dst,
                enum eCONNECTED connected){ // FOUR | EIGHT

    uint32_t width;
    uint32_t height;
    uint32_t changes = 1;
    uint32_t widthEnd, heigthEnd;
		uint16_t *pDataSrc, *pDataDst;
		
	//Markeer border pixels gelijk aan 0
	{
		// LO -> RO
		width = (src->width);
		pDataDst = ((uint16_t *)(dst->data));
		pDataSrc = ((uint16_t *)(src->data));
		
		while(width--)
		{
			if(*pDataSrc == 0){
				*pDataDst = 0x0200; 
			}
			pDataSrc++;
			pDataDst++;
		}

		// LB -> RB
		width = (src->width);
		pDataDst = ((uint16_t *)(dst->data)) + (width*src->height);
		pDataSrc = ((uint16_t *)(src->data)) + (width*src->height);
		while(width--)
		{
			if(*pDataSrc == 0){
				*pDataDst = 0x0200; 
			}
			pDataSrc--;
			pDataDst--;
		}

		// LB -> LO & RB -> RO
		width = ((src->width-1));
		height = (src->height);
		pDataSrc = (uint16_t *)src->data;
		pDataDst = (uint16_t *)dst->data;
		while(height--)
		{
			if(*pDataSrc == 0){
				*pDataDst = 0x0200; 
			}
			pDataDst+=width;
			pDataSrc+=width;
			if(*pDataSrc == 0){
				*pDataDst = 0x0200; 
			}
			pDataDst++;
			pDataSrc++;
		}
	}
	
	//Markeer background pixels
	{
		//Find border blobs
		widthEnd = (src->width);
		heigthEnd = (src->height);
		width = (src->width);

		while(changes)
		{
			changes = 0;
			pDataDst = (uint16_t *)dst->data;
			// LB -> RO
			for(height=0; height<heigthEnd; height++)
			{
				for(width=0; width<widthEnd; width++)
				{	
					if(*pDataDst == 0){
						if(iNeighbourCount(src,width,height,0x0200,connected) > 0)
						{
							*pDataDst = 0x0200; 
						}
					}
					pDataDst++;
				}
			}

			// RO -> LB
			pDataDst = ((uint16_t *)(dst->data)) + (width*src->height);
			for(height=(heigthEnd-1); height>0; height--)
			{
				for(width=(widthEnd-1); width>0; width--)
				{
					if(*pDataDst == 0){
						if(iNeighbourCount(src,width,height,0x0200,connected) > 0)
						{
							*pDataDst = 0x0200; 
							changes++;
						}
					}
					pDataDst--;
				}
			}
		}
	}
	//Set gaten op 1
	vSetSelectedToValue(src,dst,0,1);
	
	//Remove background
	vSetSelectedToValue(dst,dst,2,0);
}
void vRemoveBorderBlobs(image_t *src, // must be a binary image
                        image_t *dst,
                        enum eCONNECTED connected){ // FOUR | EIGHT

	uint32_t width;
	uint32_t height;
	uint32_t changes = 1;
	uint32_t widthEnd, heigthEnd;
	uint16_t *pDataSrc, *pDataDst;
	uint16_t temp;

	//Markeer border pixels gelijk aan 1
	{
		// LB -> RB
		width = (src->width);
		pDataDst = ((uint16_t *)(dst->data));
		pDataSrc = ((uint16_t *)(src->data));
		
		while(width--)
		{
			if(*pDataSrc == 0x0100){
				*pDataDst = 0x0200; 
			}
			pDataSrc++;
			pDataDst++;
		}

		// LO -> RO
		width = (src->width);
		pDataDst = ((uint16_t *)(dst->data)) + (width*src->height);
		pDataSrc = ((uint16_t *)(src->data)) + (width*src->height);
		while(width--)
		{
			if(*pDataSrc == 0x0100){
				*pDataDst = 0x0200; 
			}
			pDataSrc--;
			pDataDst--;
		}

		// LB -> LO & RB -> RO
		width = ((src->width)-1);
		height = (src->height);
		pDataSrc = (uint16_t *)src->data;
		pDataDst = (uint16_t *)dst->data;
		while(height--)
		{
			if(*pDataSrc == 0x0100){
				*pDataDst = 0x0200; 
			}
			pDataDst+=width;
			pDataSrc+=width;
			if(*pDataSrc == 0x0100){
				*pDataDst = 0x0200; 
			}
			pDataDst++;
			pDataSrc++;
		}
	}
	//Find border blobs
	{
		widthEnd = (src->width)+1;
		heigthEnd = (src->height)+1;
		
		pDataSrc = (uint16_t *)src->data;
		pDataDst = (uint16_t *)dst->data;

		while(changes)
		{
			//changes = 0;
			// LB -> RO
			for(height=1; height<heigthEnd; height++)
			{
				for(width=1; width<widthEnd; width++)
				{
					temp = (width*height);
					if(pDataSrc[temp] == 0x0100){
						if(iNeighbourCount(src,width,height,0x0200,connected) > 0)
						{
							pDataDst[temp] = 0x0200; 
						}
					}
				}
			}
			changes = 0;
			// RO -> LB
			for(height=(heigthEnd-1); height>1; height--)
			{
				for(width=(widthEnd-1); width>1; width--)
				{
					temp = (width*height);
					if(pDataSrc[temp] == 0x0100){
						if(iNeighbourCount(src,width,height,0x0200,connected) > 0)
						{
							pDataDst[temp] = 0x0200;
							changes++;							
						}
					}
				}
			}
		}
	}
	
	//remove border blobs
	vSetSelectedToValue(dst,dst,2,0);
}

uint32_t iLabelBlobs(image_t *img, // must be a binary image
                     enum eCONNECTED connected){
	uint16_t blobCnt = 0x1;
	uint32_t changes = 1;
	uint32_t lowest = 0x0001;
	uint32_t widthEnd = img->width,
					heigthEnd = img->height;
	uint32_t width;
	uint32_t height;
	uint32_t pixels;
	uint16_t temp;
	uint16_t *pData1, *pData2;

	pData1 = (uint16_t*) img->data;
	
	//Maak objecten 1 -> 255
	vSetSelectedToValue(img,img,1,255);
	
	//Label blobs, eerste ronde
	// LB -> RO
	pData1 = ((uint16_t*) img->data);
	pData2 = ((uint16_t*) img->data) + (img->width);
	
	for(height=1; height<heigthEnd; height++){	
		for(width=1; width<widthEnd; width++){
			if(*pData2 == 0xFF00){ // no background
				if(connected == FOUR){
					if(*--pData2 > 0){ //west
						if(*pData1 > 0){ //north
							if(*pData1 < *pData2) //north lower than west
								*++pData2 = *pData1; //is north
							else{
								temp = *pData2;
								*++pData2 = temp; //is west
							}
						}
						else{
							temp = *pData2;
								*++pData2 = temp; //is west
						}
					}
					else{
						if(*pData1 > 0){ //north
							*++pData2 = *pData1; //is north
						}
						else{   //create new label
							*++pData2 = blobCnt;
							blobCnt+=0x1;
							if(blobCnt >= 0xFE00)
									return 0xFE00;
						}
					}
				}
				else{
					if(*--pData2 > 0){ //west
						temp = *pData2;
								*++pData2 = temp; //is west
					}
					else{
						if(*--pData1 > 0){ //north west
							*++pData2 = *pData1; //is north west
						}
						else{ 
							if(*++pData1 > 0){ //north
								*++pData2 = *pData1;
							}
							else{   //north east
								if(*++pData1 > 0){ //north east
									*++pData2 = *pData1--;
								}
								else{   //create new label
									pData1--;
									*++pData2 = blobCnt;
									blobCnt += 0x1;
									if(blobCnt >= 0xFE00) //error 
										return 0xFE00;
								}
							}
						}
					}
				}
			}
			pData1++;
			pData2++;
		}
	}
	
	//return 0;
	
	//tweede ronde
	while(changes){
			changes=0;
			
			pData1 = ((uint16_t*) img->data) + (img->width*img->height) - img->width;
			pData2 = ((uint16_t*) img->data) + (img->width*img->height);
	
			// RO -> LB
			for(height=heigthEnd-1; height>0; height--){
					for(width=widthEnd-1; width>0; width--){
							if(*pData1 > 0){ // no background
									lowest = 0xFE00;
									if(connected == FOUR){
											pData1++;
											if(*pData1 <= lowest && *pData1 > 0) //east
													lowest = *pData1;
											if(*pData2 <= lowest && *pData2 > 0) //south
													lowest = *pData2;
											pData1--;
											if(lowest != 0xFE00){
													if(*pData1 != lowest){
															*pData1 = lowest;
															changes=1;
													}
											}
											pData1--;
											pData2--;
									}
									else{
											pData1++;
											if(*pData1 <= lowest && *pData1 > 0) //east
													lowest = *pData1;
											pData1--;
											if(*pData2 <= lowest && *pData2 > 0) //south
													lowest = *pData2;
											pData2++;
											if(*pData2 <= lowest && *pData2 > 0) //south east
													lowest = *pData2;
											pData2-=2;
											if(*pData2 <= lowest && *pData2 > 0) //south west
													lowest = *pData2;
											if(lowest != 0xFE00){
													if(*pData1 != lowest){
															*pData1 = lowest;
															changes=1;
													}
											}
											pData1--;
									}
							}
							else{
								pData1--;
								pData2--;
							}
					}
			}
			
			// LB -> RO
			pData1 = ((uint16_t*) img->data);
			pData2 = ((uint16_t*) img->data) + (img->width);
			
			for(height=1; height<heigthEnd; height++){
					for(width=1; width<widthEnd; width++){
							if(*pData2 > 0){ // no background
									lowest = 0xFE00;
									if(connected == FOUR){
											if(*pData1 <= lowest && *pData1 > 0) //north
													lowest = *pData1;
											pData2--;
											if(*pData2 <= lowest && *pData2 > 0) //west
													lowest = *pData2;
											pData2++;
											if(lowest != 0xFE00){
													if(*pData2!= lowest){
															*pData2 = lowest;
															changes=1;
													}
											}
									}
									else{
											if(*pData1 <= lowest && *pData1 > 0) //north
													lowest = *pData1;
											pData2--;
											if(*pData2 <= lowest && *pData2 > 0) //west
													lowest = *pData2;
											pData2++;
											pData1++;
											if(*pData1 <= lowest && *pData1 > 0) //north east
													lowest = *pData1;
											pData1-=2;
											if(*pData1 <= lowest && *pData1 > 0) //north west
													lowest = *pData1;
											pData1+=2;
											if(lowest != 0xFE00){
													if(*pData2 != lowest){
															*pData2 = lowest;
															changes=1;
													}
											}
											pData2++;
									}
							}
							else{
								pData1++;
								pData2++;
							}
					}
			}
	}
	
	blobCnt = 0;
	pData1 = ((uint16_t*) img->data);
	
	// LB -> RO
	for(height=0; height<heigthEnd; height++){
			for(width=0; width<widthEnd; width++){
					if(*pData1 > 0){
							if(*pData1 > blobCnt)
							{
									blobCnt++;
									if(blobCnt > 254)
											blobCnt = 254;
									//Maak objecten x -> blobCnt
									
									temp = *pData1;
									pData2 = ((uint16_t*) img->data);
									pixels = (img->width*img->height);

									while(pixels--)
									{
											if(*pData2 == temp)
												*pData2 = blobCnt;
											pData2++;
									}
							}
					}
					pData1++;
			}
	}
	return blobCnt;
}
uint8_t iNeighbourCount(image_t *img,
                        uint16_t x,
                        uint16_t y,
                        uint16_t value,
                        enum eCONNECTED connected){
	uint8_t count = 0;
	int32_t width = img->width;
	int32_t height = img->height;
	int32_t xx, yy, xM, yM;
	int16_t *pData;

	//ERROR Checking
	if(x >= width)
			return 0;
	if(y >= height)
			return 0;
	
	xM = x+2;
	yM = y+2;

	for(yy = y-1; yy<yM; yy++)
	{
		for(xx = x-1; xx<xM; xx++)
		{
			if(xx >= 0){
				if(yy >= 0){
					if(xx < width){
						if(yy < height){
							if(!(xx == x && yy == y)){
								if(connected == FOUR){
									pData = ((int16_t*) img->data) + xx + yy*width;
									if(yy == y){
										if((*pData&0xFF00) == value)
											count++;
									}
									else if(xx == x){
										if((*pData&0xFF00) == value)
											count++;
									}
								}
								else{
									pData = ((int16_t*) img->data) + xx + yy*width;
									if((*pData&0xFF00) == value)
											count++;
								}
							}
						}
					}
				}
			}
		}
	}
	return count;
}
void vCentroid(image_t *img, uint8_t blobnr, uint8_t *xc, uint8_t *yc, uint32_t* m00){
    uint32_t height, width;
    uint32_t widthEnd = img->width,
            heigthEnd = img->height;
    float M00 = 0, M10=0, M01=0;
    float Xc, Yc;
		uint16_t *pData = (uint16_t*) img->data;

    // M00: sum of pixels
    // M10: sum of x values
    // M01: sum of y values

    // /x = M10/M00
    // /y = M01/M00
    // /x,/y = xc,yc = centroid = x,y positie van object

    //calculate M00, M01,M10
    for(height=0; height<heigthEnd; height++){
        for(width=0; width<widthEnd; width++){
            if(*pData==blobnr)
            {
                M00++;              //sum of pixels
                M10+=width;         //sum of x values
                M01+=height;        //sum of y values
            }
						pData++;
        }
    }

    //calculate Xc, Yc
    Xc = (M10/M00);
    Yc = (M01/M00);

    *xc = (uint8_t) Xc;
    *yc = (uint8_t) Yc;
		*m00 = (uint32_t) M00;
}
double dNormalizedCentralMoments(image_t *img, 
																uint8_t blobnr, 
																int p, 
																int q,
																uint8_t xc, 
																uint8_t yc, 
																uint32_t m00){
    uint32_t height, width;
    uint32_t widthEnd = img->width,
            heigthEnd = img->height;
		uint16_t *pData = (uint16_t*) img->data;

    double M00 = m00;
		double Xc = xc;
		double Yc = yc;
    double Upq = 0;
    double Npq = 0;
    double x, y;


    //check
    if((p==0) && (q==1))
        return 0.0f;
    if((p==1) && (q==0))
        return 0.0f;
    if((p==0) && (q==0))
        return 1.0f;

    //calculate Upq
    Upq = 0;
    if((p==1) && (q==1)){
        for(height=0; height<heigthEnd; height++){
            for(width=0; width<widthEnd; width++){
                if(*pData==blobnr)
                {
                    x = width;
                    y = height;
                    Upq += ((x-Xc) * (y-Yc));
                }
								pData++;
            }
        }
    }
    else if((p==2) && (q==0)){
        for(height=0; height<heigthEnd; height++){
            for(width=0; width<widthEnd; width++){
                if(*pData==blobnr)
                {
                    x = width;
                    Upq += pow((x-Xc),p);
                }
								pData++;
            }
        }
    }
    else if((p==0) && (q==2)){
        for(height=0; height<heigthEnd; height++){
            for(width=0; width<widthEnd; width++){
                if(*pData==blobnr)
                {
                    y = height;
                    Upq += pow((y-Yc),q);
                }
								pData++;
            }
        }
    }
    else{
        for(height=0; height<heigthEnd; height++){
            for(width=0; width<widthEnd; width++){
                if(*pData==blobnr)
                {
                    x = width;
                    y = height;
                    Upq += (pow((x-Xc),p) * pow((y-Yc),q));
                }
								pData++;
            }
        }
    }

    //calculate Y (=2)
    //calculate ncm
    //Npq = (Upq/(pow(U00,2)));
    Npq = (Upq/(M00*M00));

    //QDEBUG("Npq: " << Npq);

    //return ncm;
    return(Npq);
}


